#include "pid_app.h"
#include "math.h"

extern UART_HandleTypeDef huart1;



/*PID实例*/
PID_T  pid_cam_x;
PID_T  pid_cam_y;
//位置环PID参数结构体
PidParams_t pid_cam_x_params = {
    .kp = 0.105f,          
    .ki = 0.00f,          
    .kd = 0.00f,          
    .out_min = -999.0f,    
    .out_max = 999.0f    
};

PidParams_t pid_cam_y_params = {
    .kp = 0.105f,          
    .ki = 0.00f,          
    .kd = 0.01f,          
    .out_min = -999.0f,    
    .out_max = 999.0f    
};


void PID_Init(void)
{
  pid_init(&pid_cam_x,pid_cam_x_params.kp,pid_cam_x_params.ki,
    pid_cam_x_params.kd,0.0f,pid_cam_x_params.out_max);

  pid_init(&pid_cam_y,pid_cam_y_params.kp,pid_cam_y_params.ki,
    pid_cam_y_params.kd,0.0f,pid_cam_y_params.out_max);

    
}

uint8_t pid_run;
/**
 * @brief 摄像头云台PID调控任务
 * 
 * @param target_x 
 * @param target_y 
 * @param now_x_position 
 * @param now_y_position 
 */
float target_x;
float target_y;
float now_x_position;
float now_y_position;
void Cam_Pid_Task(void)
{

  if(pid_run == 0) 
  {
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4,0);
    //HAL_Delay(500);
    //__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4,50);
    Step_Motor_Set_Speed_my(-8,0,2);
    return;
  }

  float adjust_x = 0.0f;
  float adjust_y = 0.0f;
  //设置位置环目标值
  pid_set_target(&pid_cam_x,0);
  pid_set_target(&pid_cam_y,0);
  my_printf(&huart1,"%d,%d\r\n",0,x);
  //使用位置式pid获取当前调控量
  //my_printf(&huart5,"x:%d,y:%d\r\n",x,y);
  if(x >= -1 && x <= 1 && y>=-1 && y <= 1)
  {
      __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4,50);
  }
  else
  {
      __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4,0);
  }
  adjust_x = pid_calculate_positional(&pid_cam_x, x);
  adjust_y = pid_calculate_positional(&pid_cam_y, y);
  //输出限幅
  adjust_x = pid_constrain(adjust_x, pid_cam_x_params.out_min, pid_cam_x_params.out_max);
  adjust_y = pid_constrain(adjust_y, pid_cam_y_params.out_min, pid_cam_y_params.out_max);
 
  //将位置环调控量,作为步进电机速度环输入量输入
 // my_printf(&huart5,"adjust_x:%.2f,adjust_y:%.2f\r\n",adjust_x,adjust_y);
  Step_Motor_Set_Speed_my(adjust_x,adjust_y,1);
}




